/*
 * GPS_MTK.cpp
 *
 * Created: 01.05.2012 13:25:06
 *  Author: maxim.karban@gmail.com
 */ 

#if defined(GPS)

#include "halcyon_v1.h"

#include <avr/io.h>
#include <avr/interrupt.h>
#include <math.h>
#include <util/delay.h>
#include <avr/pgmspace.h>

#include <string.h>
#include "gps.h"
#include "uart.h"

#include "sys_events.h"

#include "debug.h"

// GGA-Global Positioning System Fixed Data, message 103,00
#define GGA_ON   "$PSRF103,00,00,01,01*25\r\n"
#define GGA_OFF  "$PSRF103,00,00,00,01*24\r\n"

// GLL-Geographic Position-Latitude/Longitude, message 103,01
#define GLL_ON   "$PSRF103,01,00,01,01*24\r\n"
#define GLL_OFF  "$PSRF103,01,00,00,01*25\r\n"

// GSA-GNSS DOP and Active Satellites, message 103,02
#define GSA_ON   "$PSRF103,02,00,01,01*27\r\n"
#define GSA_OFF  "$PSRF103,02,00,00,01*26\r\n"

// GSV-GNSS Satellites in View, message 103,03
#define GSV_ON   "$PSRF103,03,00,01,01*26\r\n"
#define GSV_OFF  "$PSRF103,03,00,00,01*27\r\n"

// RMC-Recommended Minimum Specific GNSS Data, message 103,04
#define RMC_ON   "$PSRF103,04,00,01,01*21\r\n"
#define RMC_OFF  "$PSRF103,04,00,00,01*20\r\n"

// VTG-Course Over Ground and Ground Speed, message 103,05
#define VTG_ON   "$PSRF103,05,00,01,01*20\r\n"
#define VTG_OFF  "$PSRF103,05,00,00,01*21\r\n"

// Wide Area Augmentation System, message 151
#define WAAS_ON  "$PSRF151,01*0F\r\n"
#define WAAS_OFF "$PSRF151,00*0E\r\n"


char * const initstr PROGMEM = (char * const) GGA_ON GLL_OFF GSA_OFF GSV_OFF RMC_ON VTG_OFF WAAS_OFF;

gpsData_t gpsData;

void gpsInit(void){
  //factory reset
  GPS_PIN_SETUP;
  //spi mode
  SCK_OFF;
  GPSCS_ON;

  //UART mode for GPS tests
  //SCK_ON;
  //GPSCS_ON;
  
  GPS_RESET_OFF;
  _delay_ms(200);
  GPS_RESET_ON;
  _delay_ms(1000);
  GPS_OFF;
  _delay_ms(200);
  GPS_ON;
  _delay_ms(200);
  GPS_OFF;

  GPSCS_OFF;//select gps device on SPI bus
  //reset intervals
  
  //spi pin init
  DDRB |= 1 << 3;//MOSI out
  PORTB |= 1 << 4;//MISO pull up 
  
  //spi_init
  //SPCR |= (1 << SPE) | (0x02 << SPR0) | (0 << CPOL) | (1 << CPHA) | (1 << MSTR);
  SPCR |= (1 << SPE) | (1 << MSTR) | (0 << CPOL) | (0x06 << SPR0);
  SPSR |= 1 << SPI2X;

  _delay_ms(500);
  uint8_t i = 0;
  while(initstr[i] != 0){

    SPDR = pgm_read_byte(&(initstr[i++]));

    while(!(SPSR & (1<<SPIF)));
  }
  _delay_ms(200);

  SPCR |= 1 << SPIE;
  
  //WGM mode CTC on OCR2A
  TCCR2A = (0 << COM2A1) | (0 << COM2A0) | (1 << WGM21) | (0 << WGM20) ;
  TCCR2B = (0 << WGM22) | (1 << CS22) | (0 << CS21) | (1 << CS20);

  OCR2A = 128;//~4ms
  
  TIMSK2 = 1 << OCIE2A;

}

void strtobpap(char * _str, int16_t *bp, uint16_t *ap){
  int16_t integer_part = 0;
  int16_t mult = 0;
  int16_t sign = 1;
  int16_t float_part = 0;
  int8_t sl = strlen((char *)_str);
  int8_t strpos = sl;
  for(int8_t i = 0; i < sl; i++){
    if(_str[i] == '.'){
      strpos = i;
      break;
    }
  }
  mult = 1;
  sign = 1;
  for(int8_t i = strpos - 1; i >= 0; i--){
    if((i == 0) & (_str[i] == '-')){
      sign = -1;
    } else {
      integer_part += (_str[i] - 0x30) * mult;
      mult *= 10;
    }    
  }
  mult = 1;
  for(int8_t i = sl - 1; i > strpos; i--){
    float_part += (_str[i] - 0x30) * mult;
    mult *= 10;
  }
  *bp = (int16_t)integer_part*sign;
  *ap = (int16_t)float_part;
}

int fn = 0;
int pos = 0;

uint8_t crc = 0; 

enum GpsParserState{
  GPS_START,
  GPS_HEADER,
  GPS_FIELDS,
  GPS_CRC
} gpsParserState = GPS_START;

struct smStruct_t{
  uint8_t _chr;
  SMState_t ifTrue;
  SMState_t ifFalse;
};

//state machine table
const static smStruct_t smTable[SM_LAST] PROGMEM = {
  {/*SM_START       */ '$' , SM_L_G0          , SM_IDLE_A70   },
  {/*SM_L_G0        */ 'G' , SM_L_P0          , SM_L_P1       },
  {/*SM_L_P0        */ 'P' , SM_L_R0          , SM_ERROR      },
  {/*SM_L_R0        */ 'R' , SM_L_M0          , SM_L_G1       },
  {/*SM_L_M0        */ 'M' , SM_L_C0          , SM_ERROR      },
  {/*SM_L_C0        */ 'C' , SM_RMC_COMA      , SM_ERROR      },
  {/*SM_L_G1        */ 'G' , SM_L_G2          , SM_ERROR      },
  {/*SM_L_G2        */ 'G' , SM_L_A0          , SM_L_S0       },
  {/*SM_L_A0        */ 'A' , SM_GGA_COMA      , SM_ERROR      },//"$GPGGA
  {/*SM_L_S0        */ 'S' , SM_L_A1          , SM_ERROR      },
  {/*SM_L_A1        */ 'A' , SM_ANY_LF        , SM_L_V0       },
  {/*SM_L_V0        */ 'V' , SM_ANY_LF        , SM_ANY_LF     },
  {/*SM_L_P1        */ 'P' , SM_L_S1          , SM_ERROR      },
  {/*SM_L_S1        */ 'S' , SM_L_R1          , SM_ERROR      },
  {/*SM_L_R1        */ 'R' , SM_L_F0          , SM_ERROR      },
  {/*SM_L_F0        */ 'F' , SM_L_10          , SM_ERROR      },
  {/*SM_L_10        */ '1' , SM_L_50          , SM_ERROR      },
  {/*SM_L_50        */ '5' , SM_L_00          , SM_ERROR      },
  {/*SM_L_00        */ '0' , SM_PCRF150_LF    , SM_L_60       },
  {/*SM_L_60        */ '6' , SM_PCRF156_LF    , SM_ERROR      },

  {/*SM_PCRF150_LF  */ '\n', SM_OK            , SM_PCRF150_LF },
  {/*SM_PCRF156_LF  */ '\n', SM_IDLE          , SM_PCRF156_LF },

  {/*SM_RMC_COMA    */ ',' , SM_RMC_TIME      , SM_ERROR      },
  {/*SM_RMC_TIME    */ ',' , SM_RMC_STATUS    , SM_RMC_TIME   },
  {/*SM_RMC_STATUS  */ ',' , SM_RMC_LAT       , SM_RMC_STATUS },
  {/*SM_RMC_LAT     */ ',' , SM_RMC_LAT_NS    , SM_RMC_LAT    },
  {/*SM_RMC_LAT_NS  */ ',' , SM_RMC_LON       , SM_RMC_LAT_NS },
  {/*SM_RMC_LON     */ ',' , SM_RMC_LON_EW    , SM_RMC_LON    },
  {/*SM_RMC_LON_EW  */ ',' , SM_RMC_SPEED     , SM_RMC_LON_EW },
  {/*SM_RMC_SPEED   */ ',' , SM_RMC_DIR       , SM_RMC_SPEED  },
  {/*SM_RMC_DIR     */ ',' , SM_RMC_DATE      , SM_RMC_DIR    },
  {/*SM_RMC_DATE    */ ',' , SM_RMC_MAG       , SM_RMC_DATE   },
  {/*SM_RMC_MAG     */ ',' , SM_RMC_MAG_EW    , SM_RMC_MAG    },
  {/*SM_RMC_MAG_EW  */ ',' , SM_RMC_MODE      , SM_RMC_MAG_EW },
  {/*SM_RMC_MODE    */ '*' , SM_RMC_CRC       , SM_RMC_MODE   },
  {/*SM_RMC_CRC     */ '\r', SM_RMC_LF        , SM_RMC_CRC    },
  {/*SM_RMC_LF      */ '\n', SM_OK            , SM_ERROR      },
     
  {/*SM_GGA_COMA    */ ',' , SM_GGA_TIME      , SM_ERROR      },//,
  {/*SM_GGA_TIME    */ ',' , SM_GGA_LAT       , SM_GGA_TIME   },//235946.019,
  {/*SM_GGA_LAT     */ ',' , SM_GGA_LAT_NS    , SM_GGA_LAT    },//,
  {/*SM_GGA_LAT_NS  */ ',' , SM_GGA_LON       , SM_GGA_LAT_NS },//,
  {/*SM_GGA_LON     */ ',' , SM_GGA_LON_EW    , SM_GGA_LON    },//,
  {/*SM_GGA_LON_EW  */ ',' , SM_GGA_Q         , SM_GGA_LON_EW },//,
  {/*SM_GGA_Q       */ ',' , SM_GGA_CNT       , SM_GGA_Q      },//0,
  {/*SM_GGA_CNT     */ ',' , SM_GGA_HDOP      , SM_GGA_CNT    },//00,
  {/*SM_GGA_HDOP    */ ',' , SM_GGA_ALT       , SM_GGA_HDOP   },//,
  {/*SM_GGA_ALT     */ ',' , SM_GGA_ALT_M     , SM_GGA_ALT    },//,
  {/*SM_GGA_ALT_M   */ ',' , SM_GGA_GEO       , SM_GGA_ALT_M  },//M,
  {/*SM_GGA_GEO     */ ',' , SM_GGA_GEO_M     , SM_GGA_GEO    },//0.0,
  {/*SM_GGA_GEO_M   */ ',' , SM_GGA_DIF       , SM_GGA_GEO_M  },//M,
  {/*SM_GGA_DIF     */ ',' , SM_GGA_DIF_ID    , SM_GGA_DIF    },//,
  {/*SM_GGA_DIF_ID  */ '*' , SM_GGA_CRC       , SM_GGA_DIF_ID },//0000*
  {/*SM_GGA_CRC     */ '\r', SM_GGA_LF        , SM_GGA_CRC    },//51\r
  {/*SM_GGA_LF      */ '\n', SM_OK            , SM_ERROR      },//\n";

  {/*SM_ANY_LF      */ '\n', SM_OK            , SM_ANY_LF     },

  {/*SM_IDLE_A70    */ 0xA7, SM_IDLE          , SM_IDLE_B40   },
  {/*SM_IDLE_B40    */ 0xB4, SM_IDLE          , SM_ERROR      },
  
  {/*SM_IDLE        */ ' ' , SM_STOP          , SM_STOP       },
  {/*SM_CRC_ERROR   */ ' ' , SM_STOP          , SM_STOP       },
  {/*SM_ERROR       */ ' ' , SM_STOP          , SM_STOP       },
  {/*SM_OK          */ ' ' , SM_STOP          , SM_STOP       },
  {/*SM_STOP        */ ' ' , SM_STOP          , SM_STOP       }
};

SMState_t smState;

void smProcessData(char _chr, SMState_t State){
  hLock.gpsDataLocked = true;
  switch(State){
    case SM_START:
      crc = 0x00;
      gpsData.crc_pos = 0;
    break;
    case SM_RMC_TIME: 
      //memcpy(gpsData.parser_buff, "101112.050", 7);
      gpsData.gpsNavData.time_hour_min = 
        (((gpsData.parser_buff[0] - 0x30)*10 + (gpsData.parser_buff[1] - 0x30))) | 
        (((gpsData.parser_buff[2] - 0x30)*10 + (gpsData.parser_buff[3] - 0x30)) << 8); 
      gpsData.gpsNavData.time_sec  = 
        ((gpsData.parser_buff[4] - 0x30)*10 + (gpsData.parser_buff[5] - 0x30)); 
    break;
    case SM_RMC_STATUS:
      //memcpy(gpsData.parser_buff, "A\x00", 2);
      gpsData.gpsNavData.status = gpsData.parser_buff[0];
    break;
    case SM_RMC_LAT:
      //memcpy(gpsData.parser_buff, "5321.6802\x00", 10);
      strtobpap(&gpsData.parser_buff[0], &gpsData.gpsNavData.lat_bp, &gpsData.gpsNavData.lat_ap);
    break;
    case SM_RMC_LAT_NS:
      //memcpy(gpsData.parser_buff, "S\x00", 2);
      gpsData.gpsNavData.lat_ns = gpsData.parser_buff[0];
    break;
    case SM_RMC_LON:
      //memcpy(gpsData.parser_buff, "00630.3371\x00", 11);
      strtobpap(&gpsData.parser_buff[0], &gpsData.gpsNavData.lon_bp, &gpsData.gpsNavData.lon_ap);
    break;
    case SM_RMC_LON_EW:
      //memcpy(gpsData.parser_buff, "W\x00", 2);
      gpsData.gpsNavData.lon_ew = gpsData.parser_buff[0];
    break;
    case SM_RMC_SPEED:
      //memcpy(gpsData.parser_buff, "100.0\x00", 6);
      strtobpap(&gpsData.parser_buff[0], &gpsData.gpsNavData.speed_bp, &gpsData.gpsNavData.speed_ap);
    break;
    case SM_RMC_DIR:
      //memcpy(gpsData.parser_buff, "10.0\x00", 5);
      strtobpap(&gpsData.parser_buff[0], &gpsData.gpsNavData.course_bp, &gpsData.gpsNavData.course_ap);
    break;
    case SM_RMC_DATE:
      //memcpy(gpsData.parser_buff, "101012\x00", 11);
      gpsData.gpsNavData.date_day_month  = 
        (((gpsData.parser_buff[0] - 0x30)*10 + (gpsData.parser_buff[1] - 0x30))) | 
        (((gpsData.parser_buff[2] - 0x30)*10 + (gpsData.parser_buff[3] - 0x30)) << 8); 
      gpsData.gpsNavData.date_year = 
        ((gpsData.parser_buff[4] - 0x30)*10 + (gpsData.parser_buff[5] - 0x30));
    break;
    case SM_RMC_CRC:
    case SM_GGA_CRC:
      /*char tmpcrc[2];
      tmpcrc[0]  = (crc >> 4);
      tmpcrc[1]  = (crc & 0x0F);
      if(tmpcrc[0] <= 0x09){
        tmpcrc[0] += 0x30;
      }else{
        tmpcrc[0] += 0x37;
      }  
      tmpcrc[1]  = (crc & 0x0F);
      if(tmpcrc[1] <= 0x09){
        tmpcrc[1] += 0x30;
      }else{
        tmpcrc[1] += 0x37;
      }  
      gpsData.crc_is_ok = true;
      //  (gpsData.parser_buff[0] == tmpcrc[0]) && 
      //  (gpsData.parser_buff[1] == tmpcrc[1]);
      if(gpsData.crc_is_ok){*/
        switch(State){
          case SM_RMC_CRC:
            hLock.gps_rmc_is_ready = true;
          break;
          case SM_GGA_CRC:
            hLock.gps_gga_is_ready = true;
          break;
          default:
          break;
        } 
      /*} else {
        smState = SM_CRC_ERROR;
      } */ 
    break;
    case SM_GGA_CNT:
      strtobpap(gpsData.parser_buff, &gpsData.gpsNavData.cnt, &gpsData.gpsNavData.cnt_ap);
    break;
    case SM_GGA_ALT:
      strtobpap(&gpsData.parser_buff[0], &gpsData.gpsNavData.alt_bp, &gpsData.gpsNavData.alt_ap);
    break;
    case SM_GGA_ALT_M:
      gpsData.gpsNavData.alt_measure = gpsData.parser_buff[0];
    break;
    default:
    break;
  }  
  pos = 0;
  gpsData.parser_buff[pos] = 0;
  hLock.gpsDataLocked = false;
}  

SMState_t smPrevState;

void processGPSChar(char _chr){
  bool tmpSR = false;
  if(_chr > 0x7F){//(_chr == 0xA7) | (_chr == 0xB4)){
    smState = SM_START;
    return;
  }    
  do{
    smPrevState = smState;
    uint8_t tmp_chr = pgm_read_byte(&smTable[smPrevState]._chr);

    if(tmp_chr == _chr){
      smState = (SMState_t)pgm_read_byte(&smTable[smPrevState].ifTrue); 
      tmpSR = true;
    }else{
      smState = (SMState_t)pgm_read_byte(&smTable[smPrevState].ifFalse); 
      tmpSR = false;
    }  

    if(smPrevState == smState){
      if(pos < (GPS_PARSER_BUF_SIZE-1)){
        gpsData.parser_buff[pos++] = _chr;
        gpsData.parser_buff[pos] = 0;
      }  
    } else {
      smProcessData(_chr, smPrevState);
    }
    if(tmpSR)
      break;
    else if (smState == smPrevState)
      break;
    if(smState >= SM_IDLE)
      break;
  }while(1);
  
  
//  if(((smPrevState >= SM_L_G0    ) & (smState <= SM_RMC_MODE  )) | 
//     ((smPrevState >= SM_GGA_COMA) & (smState <= SM_GGA_DIF_ID)))
  if((smPrevState > SM_START) & (_chr != '*'))
  {
    crc ^= _chr;
  }  

}



ISR(TIMER2_COMPA_vect){
  SPDR = 0x00;
}

ISR(SPI_STC_vect){//2.64ms max seems once per second, 0.07ms middle
  //debug_pin_on;
  SPCR &= ~(1 << SPIE);
  sei();
  processGPSChar(SPDR);
  if(smState>=SM_IDLE){
    crc = 0x00;
    smState = SM_START;
  }
  SPCR |= 1 << SPIE;
  //debug_pin_off;
}

#endif